/*
    Main entrypoint for the input controller.

    Coding style guide:
        Name classes in UpperCamelCase
        Interface class names prefixed with 'I'
        Name functions/methods in lowerCamelCase
        Name variables in lowerCamelCase
        Name defines/macros in CAPS_LOCK
        Begin mutator/accessor names with 'get'/'set'
        Begin boolean accessors with 'is'

*/

#include "Common.h"
#include "OS/os.h"
#include "SoftwareSerial/SoftwareSerial.h"
#include "ControllerThread.h"
#include "GpsThread.h"
#include "MotorThread.h"
#include "Motor.h"
#include "BeagleThread.h"
#include "WindSensor.h"

SoftwareSerial beagle(17, 16);

//Ugly but necessary global variables
Motor* mainMotor = new Motor(MAIN_START, MAIN_MOTOR_CONVERSION_A, MAIN_MOTOR_CONVERSION_B);
Motor* jibMotor = new Motor(JIB_START, JIB_MOTOR_CONVERSION_A, JIB_MOTOR_CONVERSION_B);
Motor* portRudderMotor = new Motor(RUDDER_START, RUDDER_MOTOR_CONVERSION_A, RUDDER_MOTOR_CONVERSION_B);
Motor* stbdRudderMotor = new Motor(RUDDER_START, RUDDER_MOTOR_CONVERSION_A, RUDDER_MOTOR_CONVERSION_B);

volatile int enableAutonomy = 0; //should be off by default
WindSensor* windSensor = new WindSensor();

enum{
    XBEE_TASK=1,
    SENSOR_TASK,
    MOTOR_TASK,
    BEAGLE_TASK,
    IMU_TASK,
    SD_TASK,
    AUTO_TASK,
    GPS_TASK
};

int xbeeTime = 55;
int sensorTime = 10;
int motorTime = 20;
int beagleTime = 30;
int idleTime = 340;
/*int gpsTime = 15;		//pin38
int imuTime = 15;		//pin39
int xbeeTime = 55;		//pin40
int sdTime = 250;		//pin42
int sensorTime = 10;		//pin43
int motorTime = 20;		//pin44
int autoTime = 35;		//pin45*/
//for 1 sec need 800 ticks
//total so far is 400
//xbee needs 2 hz
//idle is gps + auto + sd = 300
//makes xbee fixed rate
//int idleTime = 300;

const unsigned char PPP[] = {XBEE_TASK, xbeeTime, SENSOR_TASK, sensorTime, BEAGLE_TASK, beagleTime, MOTOR_TASK, motorTime,
                             IDLE, xbeeTime, SENSOR_TASK, sensorTime, BEAGLE_TASK, beagleTime, MOTOR_TASK, motorTime,
                             IDLE, idleTime,
                             XBEE_TASK, xbeeTime, SENSOR_TASK, sensorTime, BEAGLE_TASK, beagleTime, MOTOR_TASK, motorTime,
                             IDLE, xbeeTime, SENSOR_TASK, sensorTime, BEAGLE_TASK, beagleTime, MOTOR_TASK, motorTime};

//const unsigned char PPP[] = {XBEE_TASK, xbeeTime, SENSOR_TASK, sensorTime, BEAGLE_TASK, beagleTime, MOTOR_TASK, motorTime, IDLE, idleTime};
/*const unsigned char PPP[] = {IDLE,gpsTime,IDLE,autoTime,IDLE,sdTime,IDLE,imuTime,SENSOR_TASK,sensorTime,XBEE_TASK,xbeeTime,MOTOR_TASK,motorTime,
		IDLE,idleTime,IDLE,imuTime,SENSOR_TASK,sensorTime,XBEE_TASK,xbeeTime,MOTOR_TASK,motorTime
		}; // Brians Settings April 10 2012
		*/
/*const unsigned char PPP[] = {SENSOR_TASK,sensorTime,IDLE,autoTime,BEAGLE_TASK,imuTime,IDLE,sdTime,IDLE,gpsTime,XBEE_TASK,xbeeTime,MOTOR_TASK,motorTime,
		IDLE,idleTime,SENSOR_TASK,sensorTime,BEAGLE_TASK,imuTime,XBEE_TASK,xbeeTime,MOTOR_TASK,motorTime
		}; // Brians Settings April 10 2012
*/
//const unsigned char PPP[] = {BEAGLE_TASK,40,SENSOR_TASK,20,XBEE_TASK,55,MOTOR_TASK,20,IDLE,665};
const unsigned int PT = sizeof(PPP)/2;

void imuTask(){
    //Serial.println("imu");
    //for(;;){
    //    Task_Next();
   // }
}

void gpsTask(){
    //for(;;){
    //    Task_Next();
    //}
}

void autoTask(){
    //for(;;){
   //     Task_Next();
   // }
}

void sdTask(){
    //for(;;){
    //    Task_Next();
    //}
}

void xbeeTask(){
    byte buffer[17];
    size_t checksum = 0;

    for(;;){
        //Serial.println("xbee");

        if(Serial3.available() > 0){
            //Serial.println("Serial3 Available");
            byte commandByte = Serial3.read();

            if(commandByte == MOTOR_COMMAND){

                checksum = 0;
                for(int i=0; i<8; i++){
                    buffer[i] = static_cast<byte>(Serial3.read());
                    checksum += buffer[i];
                }
                buffer[8] = Serial3.read();
                buffer[9] = Serial3.read();

                size_t checksumTransmitted = static_cast<size_t>(buffer[8]) + (static_cast<size_t>(buffer[9]) << 8);
                if(checksum == checksumTransmitted){
                    digitalWrite(12, HIGH);

                    uint16_t mainCommanded = static_cast<uint16_t>(buffer[0]) + (static_cast<uint16_t>(buffer[1]) << 8);
                    uint16_t jibCommanded = static_cast<uint16_t>(buffer[2]) + (static_cast<uint16_t>(buffer[3]) << 8);
                    uint16_t portCommanded = static_cast<uint16_t>(buffer[4]) + (static_cast<uint16_t>(buffer[5]) << 8);
                    uint16_t stbdCommanded = static_cast<uint16_t>(buffer[6]) + (static_cast<uint16_t>(buffer[7]) << 8);

                    mainMotor->setCommandedPosition(mainCommanded);
                    jibMotor->setCommandedPosition(jibCommanded);
                    portRudderMotor->setCommandedPosition(portCommanded);
                    stbdRudderMotor->setCommandedPosition(stbdCommanded);

                    digitalWrite(12, LOW);
                }
            }
            else if(commandByte == STATUS){

            }
            else if(commandByte == SET_AUTONOMOUS_MODE){
                uint8_t controlByte = Serial3.read();

                if(controlByte == 1 &&enableAutonomy == 0)
                    enableAutonomy = 1;
                else if(controlByte == 0 && enableAutonomy == 1)
                    enableAutonomy = 0;

                Beagle::sendAutonomySentence(enableAutonomy);
            }
            else{
                Serial3.flush();
            }
        }

        Task_Next();
    }
}

void sensorTask(){
    size_t checksum = 0;

    for(;;){
        //Serial.println("sensor");
        windSensor->measureWindDirection();

        if(windSensor->directionChanged()){
            char windStr[20];
            checksum = 0;

            sprintf(windStr, "$WND,%i,#", windSensor->getWindDirection());
            for(size_t i=0; i<20; i++){
                if(windStr[i] != '$')
                    checksum += static_cast<size_t>(windStr[i]);
                if(windStr[i] == '#')
                    break;
            }
            char checkStr[20];
            itoa(checksum, checkStr, 10);
            char* inter = strcat(windStr, checkStr);
            char* windOut = strcat(inter, "|");
            Serial1.write(windOut);
            //Serial.println(windOut);
        }

        Task_Next();
    }
}

void beagleTask(){
    byte buffer[20];
    size_t checksum = 0;

    for(;;){
        //Serial.println("beagleTask");

        //bool check = true;
        //while(check){
        if(enableAutonomy){
            if(Serial1.available() > 0){
                char c = Serial1.read();
                if(c == 'm'){
                    //Serial.println("Beagle start byte");
                    checksum = 0;
                    for(size_t i=0; i<3; i++){
                        buffer[i] = static_cast<byte>(Serial1.read());
                        checksum += static_cast<size_t>(buffer[i]);
                        //Serial.println(buffer[i]);
                    }
                    buffer[3] = Serial1.read();
                    buffer[4] = Serial1.read();

                    size_t checksumRec = static_cast<size_t>(buffer[3]) + (static_cast<size_t>(buffer[4]) << 8);

                    //char recBuf[5];
                    //itoa(checksumRec, recBuf, 10);
                    //Serial.println(recBuf);

                    //char buf[5];
                   // itoa(buffer[2], buf, 10);
                    //Serial.println(buf);

                    if((checksum == checksumRec) && isSailPosition(buffer[0]) && isSailPosition(buffer[1]) && isRudderPosition(buffer[2])){
                        digitalWrite(12, HIGH);
                        //Serial.println("passed");

                        int16_t mainPosition = static_cast<int16_t>(buffer[0]);
                        int16_t jibPosition = static_cast<int16_t>(buffer[1]);
                        int16_t rudderPosition = static_cast<int16_t>(buffer[2]);

                        mainMotor->setCommandedPosition(mainPosition);
                        jibMotor->setCommandedPosition(jibPosition);
                        portRudderMotor->setCommandedPosition(rudderPosition-21); //Beagle sends rudder values 0-70
                        stbdRudderMotor->setCommandedPosition(rudderPosition-21);

                        digitalWrite(12, LOW);
                        Serial.println("__PASSED__");

                        char* buf = new char[10];
                        char* buf2 = new char[10];
                        itoa(portRudderMotor->getCommandedPosition(), buf, 10);
                        itoa(stbdRudderMotor->getCommandedPosition(), buf2, 10);

                        Serial.println(buf);
                        Serial.println(buf2);

                        delete[] buf;
                        delete[] buf2;

                        //check = false;
                    }
                    else{
                        Serial.println("failed");
                    }
                }
                else
                    Serial1.flush();
            }
            //else
            //    check = false;
        } //enableAutonomy

        Task_Next();
    }
}

void motorTask(){
    byte readBuffer[22];
    byte sendBuffer[10];
    size_t checksum = 0;

    for(;;){
        //Serial.println("MotorTask");

        //Read measured positions from rudder motors
        /*Wire.requestFrom(1, 18);
        _delay_ms(2); //not sure if this is necessary, but it was included in the old code

        checksum = 0;
        for(int8_t i=0; i<16; i++){
            readBuffer[i] = static_cast<byte>(Wire.read());
            checksum += readBuffer[i];
        }
        readBuffer[16] = static_cast<byte>(Wire.read());
        readBuffer[17] = static_cast<byte>(Wire.read());

        size_t checksumTransmitted = static_cast<size_t>(readBuffer[16]) + static_cast<size_t>(readBuffer[17] << 8);
        if(checksum == checksumTransmitted){
            //Leaving out the reading of PWM max and current, for now
            uint32_t portRead = static_cast<uint32_t>(readBuffer[0]) + (static_cast<uint32_t>(readBuffer[1]) << 8) + (static_cast<uint32_t>(readBuffer[2]) << 16) + (static_cast<uint32_t>(readBuffer[3]) << 24);
            uint32_t stbdRead = static_cast<uint32_t>(readBuffer[8]) + (static_cast<uint32_t>(readBuffer[9]) << 8) + (static_cast<uint32_t>(readBuffer[10]) << 16) + (static_cast<uint32_t>(readBuffer[11]) << 24);

            portRudderMotor->setMeasuredPosition(portRead);
            stbdRudderMotor->setMeasuredPosition(stbdRead);
        }

        Serial.println("endreadrudder");

        //Read measured positions from sail motors
        Wire.requestFrom(2, 18);
        _delay_ms(2);

        checksum = 0;
        for(int8_t i=0; i<16; i++){
            readBuffer[i] = static_cast<byte>(Wire.read());
            checksum += readBuffer[i];
        }
        readBuffer[16] = static_cast<byte>(Wire.read());
        readBuffer[17] = static_cast<byte>(Wire.read());

        checksumTransmitted = static_cast<size_t>(readBuffer[16]) + static_cast<size_t>(readBuffer[17] << 8);
        if(checksum == checksumTransmitted){
            uint32_t mainRead = static_cast<uint32_t>(readBuffer[0]) + (static_cast<uint32_t>(readBuffer[1]) << 8) + (static_cast<uint32_t>(readBuffer[2]) << 16) + (static_cast<uint32_t>(readBuffer[3]) << 24);
            uint32_t jibRead = static_cast<uint32_t>(readBuffer[8]) + (static_cast<uint32_t>(readBuffer[9]) << 8) + (static_cast<uint32_t>(readBuffer[10]) << 16) + (static_cast<uint32_t>(readBuffer[11]) << 24);

            mainMotor->setMeasuredPosition(mainRead);
            jibMotor->setMeasuredPosition(jibRead);
        }

        Serial.println("endreadsails");*/

        //Send positions to rudders
        Wire.beginTransmission(1);
        checksum = 0;

        sendBuffer[0] = 'g';
        checksum += static_cast<size_t>(sendBuffer[0]);

        //each of these loops only send 2 chars, but our positions are stored as 32-bit integers (as per the old code)
        //need to investigate whether or not 32-bit precision is actually needed or not
        for(uint8_t i=1; i<3; i++){
            sendBuffer[i] = static_cast<byte>(portRudderMotor->getCommandedPositionRaw() >> ((i-1)*8));
            checksum += static_cast<size_t>(sendBuffer[i]);
        }
        for(uint8_t i=3; i<5; i++){
            sendBuffer[i] = static_cast<byte>(stbdRudderMotor->getCommandedPositionRaw() >> ((i-3)*8));
            checksum += static_cast<size_t>(sendBuffer[i]);
        }
        for(uint8_t i=5; i<7; i++){
            sendBuffer[i] = static_cast<byte>(checksum >> ((i-5)*8));
        }

        //Serial.println("endrudders");

        Wire.write(sendBuffer, 7);
        Wire.endTransmission();

        //Send positions to winches
        Wire.beginTransmission(2);
        //I2c.begin();
        checksum = 0;

        sendBuffer[0] = 'g';
        checksum += static_cast<size_t>(sendBuffer[0]);

        //ditto above with regards to bit precision
        for(uint8_t i=1; i<3; i++){
            sendBuffer[i] = mainMotor->getCommandedPositionRaw() >> ((i-1)*8);
            checksum += static_cast<size_t>(sendBuffer[i]);
        }
        for(uint8_t i=3; i<5; i++){
            sendBuffer[i] = jibMotor->getCommandedPositionRaw() >> ((i-3)*8);
            checksum += static_cast<size_t>(sendBuffer[i]);
        }
        for(uint8_t i=5; i<7; i++){
            sendBuffer[i] = checksum >> ((i-5)*8);
        }

        Wire.write(sendBuffer, 7);
        Wire.endTransmission();

        //Serial.println("motorend");

        Task_Next();
    }
}

void setup(){
    wdt_disable();

    Wire.begin();
    Serial.begin(9600);
    Serial1.begin(9600); //GPS
    //beagle.begin(9600); //Beaglebone
    Serial3.begin(9600); //XBee

    //Note: I have pretty much no idea why these pins are used or are necessary
    //      Leaving this in though for legacy compatibility.
    pinMode(13,OUTPUT);
    pinMode(38,OUTPUT);
    pinMode(39,OUTPUT);
    pinMode(40,OUTPUT);
    pinMode(41,OUTPUT);
    pinMode(42,OUTPUT);
    pinMode(43,OUTPUT);
    pinMode(44,OUTPUT);
    pinMode(45,OUTPUT);
    digitalWrite(13, LOW);

    //mainMotor = new Motor(MAIN_START, MAIN_MOTOR_CONVERSION_A, MAIN_MOTOR_CONVERSION_B);
    //jibMotor = new Motor(JIB_START, JIB_MOTOR_CONVERSION_A, JIB_MOTOR_CONVERSION_B);
    //portRudderMotor = new Motor(RUDDER_START, RUDDER_MOTOR_CONVERSION_A, RUDDER_MOTOR_CONVERSION_B);
    //stbdRudderMotor = new Motor(RUDDER_START, RUDDER_MOTOR_CONVERSION_A, RUDDER_MOTOR_CONVERSION_B);

    //windSensor = new WindSensor();

    Serial.println("init");
}


void pre_rtos_init(){
    //init();
}

int r_main(){
    init();
    setup();

    Task_Create(xbeeTask, XBEE_TASK, PERIODIC, XBEE_TASK);
    Task_Create(sensorTask, SENSOR_TASK, PERIODIC, SENSOR_TASK);
    Task_Create(beagleTask, BEAGLE_TASK, PERIODIC, BEAGLE_TASK);
    Task_Create(motorTask, MOTOR_TASK, PERIODIC, MOTOR_TASK);

    /*Task_Create(imuTask, IMU_TASK, PERIODIC, IMU_TASK);
    Task_Create(gpsTask, GPS_TASK, PERIODIC, GPS_TASK);
    Task_Create(autoTask, AUTO_TASK, PERIODIC, AUTO_TASK);
    Task_Create(sdTask, SD_TASK, PERIODIC, SD_TASK);*/
}

/*uint16_t x = 0;
int main(int argc, char* argv[])
{
    init(); //init is defined somewhere in the AVR library
    setup();

    mainMotor = new Motor(MAIN_START, MAIN_MOTOR_CONVERSION_A, MAIN_MOTOR_CONVERSION_B);
    jibMotor = new Motor(JIB_START, JIB_MOTOR_CONVERSION_A, JIB_MOTOR_CONVERSION_B);
    portRudderMotor = new Motor(RUDDER_START, RUDDER_MOTOR_CONVERSION_A, RUDDER_MOTOR_CONVERSION_B);
    stbdRudderMotor = new Motor(RUDDER_START, RUDDER_MOTOR_CONVERSION_A, RUDDER_MOTOR_CONVERSION_B);

    scheduler = new Scheduler();

    //tinyGps = new TinyGPSPlus();
    windSensor = new WindSensor();
    //compass = new Compass();

    controller = new ControllerThread(mainMotor, jibMotor, portRudderMotor, stbdRudderMotor);
    motors = new MotorThread(mainMotor, jibMotor, portRudderMotor, stbdRudderMotor);
    beagle = new BeagleThread(mainMotor, jibMotor, portRudderMotor, stbdRudderMotor, windSensor);
    //gps = new GpsThread(tinyGps);

    //must add threads in order of desired execution, ie highest priority first
    //originally I was going to have the scheduler sort threads by priority, but that seems wasteful since we will
    //only ever have 4-5 threads
    //scheduler->addThread(controller);
    //scheduler->addThread(beagle);
    //scheduler->addThread(motors);
    //scheduler->addThread(gps);

    //We need to wait for everything to be allocated before we start interrupting the cpu
    //TIMSK3 = _BV(OCIE3A); //enable compare interrupt
    sei();

    unsigned long lastTime = 0;

    unsigned long now = millis();
    while(1){
        //if((millis() - now) >= 500){
            controller->run();
            //gps->run();
            if(enableAutonomy == 1){
                beagle->run();
            }
            motors->run();
            mainMotor->setCommandedPosition(x);
            jibMotor->setCommandedPosition(x);
            if(x < 90) x++;

                char dbg[20];
                itoa(mainMotor->getCommandedPosition(), dbg, 10);
                Serial.write(dbg);

            now = millis();
        //}
        //scheduler->runCurrentThread();
    }

    //We'll probably never actually get here, since the boat is more likely to be switched off before the loop ever breaks
    //But we'll include memory clean-up just in case
    delete motors;
    delete controller;
    delete scheduler;
    delete stbdRudderMotor;
    delete portRudderMotor;
    delete jibMotor;
    delete mainMotor;

    return 0;
}

ISR(TIMER3_COMPA_vect)
{
    //scheduler->nextThread(); //set the next thread for execution
}*/
